#include "CyberryPotter.h"
#include "weights.h"
#include "nnom.h"
#include "stm32f10x.h"
#include <string.h>

extern Cyberry_Potter_Status_Typedef Cyberry_Potter_Status;
// extern uint16_t IMU_Data_ACC[3*IMU_SEQUENCE_LENGTH_MAX];
// extern float IMU_Data_ACC[3*IMU_SEQUENCE_LENGTH_MAX];
extern float IMU_Data_mGyro[IMU_SEQUENCE_LENGTH_MAX][3];
extern float IMU_Data_mAcc[IMU_SEQUENCE_LENGTH_MAX][3];
int8_t model_output = -1;
#define SCALE (pow(2, INPUT_1_OUTPUT_DEC))

typedef enum eModel_Output
{
    Unrecognized = -1,
    RightAngle = 0,
    SharpAngle = 1,
    Lightning = 2,
    Triangle = 3,
    Letter_h = 4,
    letter_R = 5,
    letter_W = 6,
    letter_phi = 7,
    Circle = 8,
    UpAndDown = 9,
    Horn = 10,
    Wave = 11,
    NoMotion = 12
} eModel_Output;

#ifdef NNOM_USING_STATIC_MEMORY
uint8_t static_buf[1024 * 8];
#endif // NNOM_USING_STATIC_MEMORY

void model_feed_data(void)
{
    const double scale = SCALE;
    uint16_t i = 0;
    for (i = 0; i < IMU_SEQUENCE_LENGTH_MAX; i++)
    {
        nnom_input_data[i * 3] = (int8_t)round(IMU_Data_mGyro[i][0] * scale); // 这里单片机推理时使用的应该是加速度值，训练数据时用时前三列值也就是加速度值，之前的推理数据IMU_Data_mGyro是角速度
        nnom_input_data[i * 3 + 1] = (int8_t)round(IMU_Data_mGyro[i][1] * scale);
        nnom_input_data[i * 3 + 2] = (int8_t)round(IMU_Data_mGyro[i][2] * scale);
    }
}

int8_t model_get_output(void)
{
    uint8_t i = 0;
    int8_t max_output = -128;
    int8_t ret = 0;
    for (i = 0; i < 13; i++)
    {
        printf("Output[%d] = %.2f %%\n", i, (nnom_output_data[i] / 127.0) * 100);
        if (nnom_output_data[i] >= max_output)
        {
            max_output = nnom_output_data[i];
            ret = i;
        }
    }
    if (max_output >= OUPUT_THRESHOLD)
    {
        ret = ret;
    }
    else
    {
        ret = -1;
    }

    switch (ret)
    {
    case Unrecognized:
        printf("Unrecognized");
        break;
    case RightAngle:
        printf("RightAngle");
        break;
    case SharpAngle:
        printf("SharpAngle");
        break;
    case Lightning:
        printf("Lightning");
        break;
    case Triangle:
        printf("Triangle");
        break;
    case Letter_h:
        printf("Letter_h");
        break;
    case letter_R:
        printf("Letter_R");
        break;
    case letter_W:
        printf("Letter_W");
        break;
    case letter_phi:
        printf("Letter_phi");
        break;
    case Circle:
        printf("Circle");
        break;
    case UpAndDown:
        printf("UpAndDown");
        break;
    case Horn:
        printf("Horn");
        break;
    case Wave:
        printf("Wave");
        break;
    case NoMotion:
        printf("Unrecognized");
        break;
    }
    printf("\n");
    return ret;
}


void UART3_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;
    USART_InitTypeDef USART_InitStruct;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);

    // PB10 - TX
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOB, &GPIO_InitStruct);

    // PB11 - RX
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_11;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOB, &GPIO_InitStruct);

    USART_InitStruct.USART_BaudRate = 115200;
    USART_InitStruct.USART_WordLength = USART_WordLength_8b;
    USART_InitStruct.USART_StopBits = USART_StopBits_1;
    USART_InitStruct.USART_Parity = USART_Parity_No;
    USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;

    USART_Init(USART3, &USART_InitStruct);
    USART_Cmd(USART3, ENABLE);
}

// 退出透传模式
void ESP_Exit_Transparent_Mode(void)
{
    printf("Exiting transparent mode...\n");

    Delay_ms(1200);
    USART_SendData(USART2, '+');
    while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET)
        ;
    USART_SendData(USART2, '+');
    while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET)
        ;
    USART_SendData(USART2, '+');
    while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET)
        ;
    Delay_ms(1200);

    printf("Sent +++ to exit transparent mode.\n");
}

void ESP_UART_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;
    USART_InitTypeDef USART_InitStruct;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);

    // PA2 - TX
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOA, &GPIO_InitStruct);

    // PA3 - RX
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_3;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOA, &GPIO_InitStruct);

    USART_InitStruct.USART_BaudRate = 115200;
    USART_InitStruct.USART_WordLength = USART_WordLength_8b;
    USART_InitStruct.USART_StopBits = USART_StopBits_1;
    USART_InitStruct.USART_Parity = USART_Parity_No;
    USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;

    USART_Init(USART2, &USART_InitStruct);
    USART_Cmd(USART2, ENABLE);
}

void ESP_SendString(const char *cmd)
{
    const char *p = cmd;
    while (*p)
    {
        USART_SendData(USART2, *p++);
        while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET)
            ;
    }

    USART_SendData(USART2, '\r');
    while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET)
        ;
    USART_SendData(USART2, '\n');
    while (USART_GetFlagStatus(USART2, USART_FLAG_TC) == RESET)
        ;

    Delay_ms(800);
}

void ESP_MQTT_Init(void)
{
    printf("Initializing MQTT...\r\n");

    ESP_SendString("AT"); // 测试命令
    Delay_ms(1000);

    // 设置 MQTT 用户配置
    ESP_SendString("AT+MQTTUSERCFG=0,1,\"ESP8266_Client\",\"\",\"\",0,0,\"\"");
    Delay_ms(1200);

    // 连接到服务器
    ESP_SendString("AT+MQTTCONN=0,\"dev.itmojun.com\",1883,0");
    Delay_ms(1500);

    // 订阅主题
    ESP_SendString("AT+MQTTSUB=0,\"itmojun/state/smart_plug_1\",1");
    Delay_ms(1000);

    printf("MQTT init done.\r\n");
}

void MQTT_Publish_On(void)
{
    ESP_SendString("AT+MQTTPUB=0,\"itmojun/smart_plug/cmd/1\",\"a1\",0,0");
}

void MQTT_Publish_Off(void)
{
    ESP_SendString("AT+MQTTPUB=0,\"itmojun/smart_plug/cmd/1\",\"b1\",0,0");
}

void MQTT_Light_On(void)
{
    ESP_SendString("AT+MQTTPUB=0,\"itmojun/cmd\",\"e\",0,0");
}

void MQTT_Light_Off(void)
{
    ESP_SendString("AT+MQTTPUB=0,\"itmojun/cmd\",\"f\",0,0");
}

void IR_SendString(const char *str)
{
    while (*str)
    {
        USART_SendData(USART3, *str++);
        while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET);
    }
}

void IR_Learn(void)
{
    printf(">>> IR Learning: STUDY=106\n");
    IR_SendString("STUDY=106"); // 发送命令
    Delay_ms(50);

    printf("Waiting for user to press remote (max 12s)...\n");
    char response[5] = {0};
    uint8_t idx = 0;

    for (uint16_t t = 0; t < 12000; t += 10) // 循环等待，总共12秒
    {
        if (USART_GetFlagStatus(USART3, USART_FLAG_RXNE) != RESET)
        {
            char c = USART_ReceiveData(USART3);
            response[idx++] = c;
            if (idx >= 4)
                break; // 收到 4 个字节就退出
        }
        Delay_ms(10);
    }

    if (idx >= 4 && strncmp(response, "+OK\n", 4) == 0)
        printf("IR Learning successful!\n");
    else
        printf("IR Learning failed or timeout!\n");
}

void IR_Transmit(void)
{
    printf(">>> IR Transmit: TRANSMIT=106\n");
    IR_SendString("TRANSMIT=106"); // 发送命令
    Delay_ms(100);                 // 确保串口发送完成

    char response[5] = {0};
    uint8_t idx = 0;

    for (uint16_t t = 0; t < 3000; t += 10) // 循环等待，总共3秒
    {
        if (USART_GetFlagStatus(USART3, USART_FLAG_RXNE) != RESET)
        {
            char c = USART_ReceiveData(USART3);
            response[idx++] = c;
            if (idx >= 4)
                break; // 收到 4 个字节就退出
        }
        Delay_ms(10);
    }

    if (idx >= 4 && strncmp(response, "+OK\n", 4) == 0)
        printf("IR Transmit successful!\n");
    else
        printf("IR Transmit failed!\n");
}

void ESP_Publish(const char *topic, const char *payload)
{
    char cmd[128];
    sprintf(cmd, "AT+MQTTPUB=0,\"%s\",\"%s\",0,0", topic, payload);
    ESP_SendString(cmd);
    Delay_ms(800);
}

void Model_LED(int8_t model_output)
{
    switch (model_output)
    {
    case RightAngle:
        // GPIO_WriteBit(GPIOA,GPIO_Pin_6,Bit_SET);
        IR_Transmit(); // 发射第1个学习码
        break;
    case letter_R:
        // GPIO_WriteBit(GPIOA,GPIO_Pin_6,Bit_RESET);
        // ESP_SendString("AT+MQTTPUB=0,\"itmojun/smart_plug/cmd/1\",\"a1\",0,0\r\n");
        // MQTT_Publish_On();
        ESP_Publish("itmojun/smart_plug/cmd/1", "a1");
        printf("MQTT: a1 (open fan)\n");
        // Delay_s(2);
        // printf("MQTT: a1 (open fan)\n");
        break;
    case Lightning:
        // GPIO_WriteBit(GPIOA,GPIO_Pin_5,Bit_SET);
        // ESP_SendString("AT+MQTTPUB=0,\"itmojun/smart_plug/cmd/1\",\"b1\",0,0\r\n");
        // MQTT_Publish_Off();
        ESP_Publish("itmojun/smart_plug/cmd/1", "b1");
        printf("MQTT: b1 (close fan)\n");
        // Delay_s(2);
        // printf("MQTT: b1 (close fan)\n");
        break;
    case Triangle:
        // GPIO_WriteBit(GPIOA,GPIO_Pin_5,Bit_RESET);
        IR_Transmit();
        Delay_ms(1500);
        IR_Transmit();
        break;
    case Letter_h:
        GPIO_WriteBit(GPIOA, GPIO_Pin_4, Bit_SET);
        break;
        //		case letter_R:
        //			GPIO_WriteBit(GPIOA,GPIO_Pin_4,Bit_RESET);
        //			break;
    case letter_W:
        // ESP_SendString("AT+MQTTPUB=0,\"itmojun/cmd\",\"e\",0,0\r\n");
        // MQTT_Light_On();
        // Delay_s(1);
        // printf("MQTT: e (open light)\n");
        ESP_Publish("itmojun/cmd", "e");
        printf("MQTT: e (open light)\n");
        break;
    case letter_phi:
        // ESP_SendString("AT+MQTTPUB=0,\"itmojun/cmd\",\"f\",0,0\r\n");
        // MQTT_Light_Off();
        // Delay_s(1);
        // printf("MQTT: f (close light)\n");
        ESP_Publish("itmojun/cmd", "f");
        printf("MQTT: f (close light)\n");
        break;
    }
}

void Key_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);

    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_15;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; // 上拉输入
    GPIO_Init(GPIOC, &GPIO_InitStructure);
}

int main(void)
{
    System_Init();
    UART3_Init();
    ESP_UART_Init();
    Key_Init();
    printf("Hello!\n");

    ESP_Exit_Transparent_Mode();

    ESP_MQTT_Init(); // 连接MQTT服务器

    GPIO_WriteBit(GPIOA, GPIO_Pin_7, Bit_SET);
#ifdef NNOM_USING_STATIC_MEMORY
    nnom_set_static_buf(static_buf, sizeof(static_buf));
#endif // NNOM_USING_STATIC_MEMORY
    nnom_model_t *model;
    model = nnom_model_create();

    while (1)
    {
        if (Cyberry_Potter_Status.Button_Status == BUTTON_HOLD && Cyberry_Potter_Status.IMU_Status == IMU_Idle)
        {
            IMU_Sample_Start();
            // printf("HOLD\n");
        }
        else if (Cyberry_Potter_Status.Button_Status != BUTTON_IDLE)
        {
            Cyberry_Potter_Status.Button_Status = BUTTON_IDLE;
        }
				
				
				// 测试
        // PC13 按下 → 开灯
        if (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13) == 0)
        {
            Delay_ms(20);
            if (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13) == 0)
            {
                ESP_Publish("itmojun/smart_plug/cmd/1", "a1");
                printf("MQTT: e (open light)\n");
                while (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13) == 0);
            }
        }

        // PC15 按下 → 关灯
        if (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_15) == 0)
        {
            Delay_ms(20);
            if (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_15) == 0)
            {
                ESP_Publish("itmojun/smart_plug/cmd/1", "b1");
                printf("MQTT: f (close light)\n");
                while (GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_15) == 0);
            }
        }

        if (Cyberry_Potter_Status.IMU_Status == IMU_Sampled)
        {
            LED_ON;
#ifdef SYSTEM_MODE_DATA_COLLECT
            Delay_ms(200);
            IMU_Data_Print();
#endif // SYSTEM_MODE_DATA_COLLECT

#ifndef SYSTEM_MODE_DATA_COLLECT
            model_feed_data();
            model_run(model);
            model_output = model_get_output();
            printf("%d", model_output);
            Model_LED(model_output);
// Model_MQTT_Control(model_output);
#endif // SYSTEM_MODE_DATA_COLLECT
            Cyberry_Potter_Status.IMU_Status = IMU_Idle;
            EXTI_Restore();
        }
        //		if (Cyberry_Potter_Status.IMU_Status == IMU_Sampled)
        //        {
        //            model_feed_data();
        //            uint32_t label; float prob;
        //            nnom_predict(model, &label, &prob);
        //            model_output = (int8_t)label;

        //            model_get_output();
        //            Model_LED(model_output);  // 发送红外命令

        //            Cyberry_Potter_Status.IMU_Status = IMU_Idle;
        //            EXTI_Restore();
        //        }
    }
}
